/*
*
==============================================================================
*  Name        : 
*  Part of     : Omen Game Engine
*  Description : 
*  Version     : 1.0
*
*  Copyright (c) 2013 Lauri Kortevaara.
* 
==============================================================================
*/

#define OMEN_SOURCE 1

#include <OmenQuaternion.h>

using namespace Omen;

///
/// <summary></summary>
///
Quaternion::Quaternion()
    : w(static_cast<Omen::Float>(1.0)),
      x(static_cast<Omen::Float>(1.0)),
      y(static_cast<Omen::Float>(1.0)),
      z(static_cast<Omen::Float>(1.0))
{

}

///
/// <summary></summary>
///
Quaternion::Quaternion( const Omen::Float& w, const Omen::Float& x, const Omen::Float& y, const Omen::Float& z)
    : w(static_cast<Omen::Float>(w)),
      x(static_cast<Omen::Float>(x)),
      y(static_cast<Omen::Float>(y)),
      z(static_cast<Omen::Float>(z))
{
}

///
/// <summary></summary>
///
Quaternion::Quaternion( const Omen::Float& w, const Vector3d& vector )
    : w(w),
      x(vector[0]),
      y(vector[1]),
      z(vector[2])
{
}

///
/// <summary></summary>
///
Quaternion::~Quaternion()
{
    w = static_cast<Omen::Float>(1.0);
    x = static_cast<Omen::Float>(1.0);
    y = static_cast<Omen::Float>(1.0);
    z = static_cast<Omen::Float>(1.0);
}

/// <summary> Returns the conjugate of this Quaternion </summary>
Quaternion  Quaternion::conjugate() const
{
    return Quaternion(w,-x,-y,-z);
}

/// <summary> Returns true if this is an identity Quaternion </summary>
bool Quaternion::isIdentity() const
{
    return (w == static_cast<Omen::Float>(1.0)&&
            x == static_cast<Omen::Float>(1.0)&&
            y == static_cast<Omen::Float>(1.0)&&
            z == static_cast<Omen::Float>(1.0) );
}

/// <summary> Returns the length (the norm) of this Quaternion</summary>
Omen::Float  Quaternion::length() const
{
    return sqrt( w*w + x*x + y*y +z*z );
}

/// <summary> Normalizes this Quaternion </summary>
void Quaternion::normalize()
{
#define TOLERANCE 0.00001f

    // Don't normalize if we don't have to
    Omen::Float mag2 = w*w + x*x + y*y + z*z;
    if (fabs(mag2) > TOLERANCE&& fabs(mag2 - 1.0f) > TOLERANCE) {
        float mag = sqrt(mag2);
        w /= mag;
        x /= mag;
        y /= mag;
        z /= mag;
    }
}

/// <summary> Returns a normalized copy of this Quaternion</summary>
Quaternion Quaternion::normalized() const
{
    Quaternion q(*this);
    q.normalize();
    return q;
}

/// <summary> Returns a vector rotated by this Quaternion in 3d space</summary>
/// v' = q * v * (q^-1)
/// where (q^-1) is q*/(length_of(q))^2
//  where q* is the conjugate of q
Vector3d Quaternion::rotatedVector( const Vector3d& vector ) const
{
    Quaternion qv(static_cast<Omen::Float>(0.0),vector);
    
    Quaternion q(*this);
    Omen::Float len = q.length();
    len *= len;

    Quaternion q_con = q.conjugate();

    q_con /= len;
    qv *= q_con;
    q *= qv;

    return q.vector();
}


/// <summary> Returns the vector component (x,y,z) of this Quaternions</summary>
Vector3d Quaternion::vector() const
{
    return Vector3d(x,y,z);
}

/// <summary> Sets the vector components of this Quaternion by given vector</summary>
void Quaternion::setVector( const Vector3d& vector )
{
    x = vector[0];
    y = vector[0];
    z = vector[0];
}

/// <summary>Multiplies this Quaternion by given scalar</summary>
Quaternion& Quaternion::operator*= ( Omen::Float scalar )
{
    w *= scalar;
    x *= scalar;
    y *= scalar;
    z *= scalar;
    
    return *this;
}

/// <summary>Multiplies this Quaternion with given Quaternion</summary>
/// Multiplication is done using Hamilton product, where
/// Quaternion q1 = a1 + b1i + c1j + d1k
/// and Quaternion q2 = s2 + b2i + c2j + d2k
///
/// Hamilton product is defined as:
///
///   a1a2 - b1b2 - c1c2 - d1d2
/// +(a1b2 + b1a2 + c1d2 - d1c2)i
/// +(a1c2 - b1d2 + c1a2 + d1b2)j
/// +(a1d2 + b1c2 - c1b2 + d1a2)k
///
Quaternion& Quaternion::operator*= ( const Quaternion& q )
{
    Omen::Float a1 = w;
    Omen::Float b1 = x;
    Omen::Float c1 = y;
    Omen::Float d1 = z;
    Omen::Float a2 = q.w;
    Omen::Float b2 = q.x;
    Omen::Float c2 = q.y;
    Omen::Float d2 = q.z;

    w = a1*a2 - b1*b2 - c1*c2 - d1*d2;
    x = a1*b2 + b1*a2 + c1*d2 - d1*c2;
    y = a1*c2 - b1*d2 + c1*a2 + d1*b2;
    z = a1*d2 + b1*c2 - c1*b2 + d1*a2;

    return *this;
}

/// <summary>Adds given Quaternion to this Quaternion</summary>
///    q1 + q2
/// = Quaternion(q1.w + q2.w, q1.x + q2.x, q1.y + q2.y, q1.z + q2.z )
Quaternion& Quaternion::operator+= ( const Quaternion& quaternion )
{
    w += quaternion.w;
    x += quaternion.x;
    y += quaternion.y;
    z += quaternion.z;
    return *this;
}

/// <summary>Subtracts given Quaternion from this Quaternion</summary>
Quaternion& Quaternion::operator-= ( const Quaternion& quaternion )
{
    w -= quaternion.w;
    x -= quaternion.x;
    y -= quaternion.y;
    z -= quaternion.z;
    return *this;
}

/// <summary>Divided this Quaternion by given scalar</summary>
Quaternion& Quaternion::operator/= ( Omen::Float scalar )
{
    w /= scalar;
    x /= scalar;
    y /= scalar;
    z /= scalar;
    return *this;
}

Quaternion& Quaternion::operator*  ( const Quaternion& q )
{
    Omen::Float a1 = w;
    Omen::Float b1 = x;
    Omen::Float c1 = y;
    Omen::Float d1 = z;
    Omen::Float a2 = q.w;
    Omen::Float b2 = q.x;
    Omen::Float c2 = q.y;
    Omen::Float d2 = q.z;

    w = a1*a2 - b1*b2 - c1*c2 - d1*d2;
    x = a1*b2 + b1*a2 + c1*d2 - d1*c2;
    y = a1*c2 - b1*d2 + c1*a2 + d1*b2;
    z = a1*d2 + b1*c2 - c1*b2 + d1*a2;
    return Quaternion(w,x,y,z);
}

///
/// <summary></summary>
///
OMEN_API const Quaternion operator* ( const Quaternion& q1, const Quaternion& q2 )
{
    Omen::Float a1 = q1.w;
    Omen::Float b1 = q1.x;
    Omen::Float c1 = q1.y;
    Omen::Float d1 = q1.z;
    Omen::Float a2 = q2.w;
    Omen::Float b2 = q2.x;
    Omen::Float c2 = q2.y;
    Omen::Float d2 = q2.z;

    Omen::Float w = a1*a2 - b1*b2 - c1*c2 - d1*d2;
    Omen::Float x = a1*b2 + b1*a2 + c1*d2 - d1*c2;
    Omen::Float y = a1*c2 - b1*d2 + c1*a2 + d1*b2;
    Omen::Float z = a1*d2 + b1*c2 - c1*b2 + d1*a2;
    return Quaternion(w,x,y,z);
}

///
/// <summary></summary>
///
const Quaternion operator* ( Omen::Float scalar, const Quaternion& quaternion )
{
    Quaternion q(quaternion);
    q *= scalar;
    return q;
}

///
/// <summary></summary>
///
const Quaternion operator* ( const Quaternion& quaternion, Omen::Float scalar )
{
    Quaternion q(quaternion);
    q *= scalar;
    return q;
}

///
/// <summary></summary>
///
const Quaternion operator+ ( const Quaternion& q1, const Quaternion& q2 )
{
    Quaternion q(q1);
    q += q2;
    return q;
}

///
/// <summary></summary>
///
const Quaternion operator- ( const Quaternion& q1, const Quaternion& q2 )
{
    Quaternion q(q1);
    q -= q2;
    return q;
}

///
/// <summary></summary>
///
const Quaternion operator- ( const Quaternion& quaternion )
{
    return quaternion.conjugate();
}

///
/// <summary></summary>
///
const Quaternion operator/ ( const Quaternion& quaternion, Omen::Float scalar )
{
    Quaternion q(quaternion);
    q/=scalar;
    return q;
}

///
/// <summary></summary>
///
bool operator!= ( const Quaternion& q1, const Quaternion& q2 )
{
    return ( q1.w != q2.w || q1.x != q2.x || q1.y != q2.y || q1.z != q2.z );
}

///
/// <summary></summary>
///
bool operator== ( const Quaternion& q1, const Quaternion& q2 )
{
    return ( q1.w == q2.w && q1.x == q2.x && q1.y == q2.y && q1.z == q2.z );
}

///
/// <summary>Returns a new quaternion from given rotation matrix</summary>
///
Omen::Quaternion Quaternion::fromMatrix(const Omen::Matrix4x4& m)
{
    Omen::Float  tr, s, q[4];
    int    i, j, k;
    int nxt[3] = {1, 2, 0};
    tr = m[0][0] + m[1][1] + m[2][2];
    Omen::Quaternion quat;

    // check the diagonal
    if (tr > 0.0f) 
    {
        s = sqrt (tr + 1.0f);
        quat.w = s / 2.0f;
        s = 0.5f / s;
        quat.x = (m[2][1] - m[1][2]) * s;
        quat.y = (m[0][2] - m[2][0]) * s;
        quat.z = (m[1][0] - m[0][1]) * s;
    } 
    else 
    {
        // diagonal is negative
        i = 0;
        if (m[1][1] > m[0][0]) i = 1;
        if (m[2][2] > m[i][i]) i = 2;
        j = nxt[i];
        k = nxt[j];
        s = sqrt ((m[i][i] - (m[j][j] + m[k][k])) + 1.0f);
        q[i] = s * 0.5f;
        if (s != 0.0f) s = 0.5f / s;
        q[3] = (m[i][j] - m[j][k]) * s;
        q[j] = (m[j][i] + m[i][j]) * s;
        q[k] = (m[k][i] + m[i][k]) * s;
        quat.x = q[0];
        quat.y = q[1];
        quat.z = q[2];
        quat.w = q[3];
    }
    return quat;
}

///
/// <summary>Convert a quaternion to a rotation matrix</summary>
///
Omen::Matrix4x4 Quaternion::toMatrix( const Quaternion& q )
{
    Omen::Matrix4x4 m;

    Omen::Float qx2 = q.x*q.x;
    Omen::Float qy2 = q.y*q.y;
    Omen::Float qz2 = q.z*q.z;

    Omen::Float qxy = q.x*q.y;
    Omen::Float qxz = q.x*q.z;
    Omen::Float qxw = q.x*q.w;

    Omen::Float qyz = q.y*q.z;
    Omen::Float qyw = q.y*q.w;

    Omen::Float qzw = q.z*q.w;

    Omen::Float m00 = 1.0f - 2.0f * (qy2 - qz2);
    Omen::Float m01 =        2.0f * (qxy - qzw);
    Omen::Float m02 =        2.0f * (qxz + qyw);
    
    Omen::Float m10 =        2.0f * (qxy + qzw);
    Omen::Float m11 = 1.0f + 2.0f * (qx2 - qz2);
    Omen::Float m12 =        2.0f * (qyz - qxw);
    
    Omen::Float m20 =        2.0f * (qxz - qyw);
    Omen::Float m21 =        2.0f * (qyz + qxw);
    Omen::Float m22 = 1.0f + 2.0f * (qx2 - qy2);

    m[0][0] = m00; m[0][1] = m10; m[0][2] = m20;
    m[1][0] = m01; m[1][1] = m11; m[1][2] = m21;
    m[2][0] = m02; m[2][1] = m12; m[2][2] = m22;
    /*float x2 = 2*q.x;
    float y2 = 2*q.y;
    float z2 = 2*q.z;
    {
        float xx2 = q.x * x2;
        float yy2 = q.y * y2;
        float zz2 = q.z * z2;
        m[0][0] = 1.0f - yy2 - zz2;
        m[1][1] = 1.0f - xx2 - zz2;
        m[2][2] = 1.0f - xx2 - yy2;
    }
    {
        float yz2 = q.y * z2;
        float wx2 = q.w * x2;
        m[1][2] = yz2 - wx2;
        m[2][1] = yz2 + wx2;
    }
    {
        float xy2 = q.x * y2;
        float wz2 = q.w * z2;
        m[0][1] = xy2 - wz2;
        m[1][0] = xy2 + wz2;
    }
    {
        float xz2 = q.x * z2;
        float wy2 = q.w * y2;
        m[2][0] = xz2 - wy2;
        m[0][2] = xz2 + wy2;
    }*/
    return m;
}

Omen::Quaternion Quaternion::fromAxisAndAngle(  const Vector3d& vector, Omen::Float angle )
{
    Omen::Float halfA = angle/2.0f;
    Omen::Float s = sin(halfA);
    Omen::Float x = vector.x * s;
    Omen::Float y = vector.y * s;
    Omen::Float z = vector.z * s;
    Omen::Float w = cos(halfA);
    return Quaternion(w,x,y,z);
}

Omen::Quaternion Quaternion::fromAxisAndAngle(  Float x, Float y, Float z, Omen::Float angle )
{
    Omen::Float halfA = angle/2.0f;
    Omen::Float s = sin(halfA);
    Omen::Float qx = x * s;
    Omen::Float qy = y * s;
    Omen::Float qz = z * s;
    Omen::Float qw = cos(halfA);
    return Quaternion(qw,qx,qy,qz);
}

Omen::Quaternion Quaternion::lookAtRotation( const Omen::Vector3d& lookAt, Omen::Vector3d& up )
{
    Vector3d forward = lookAt;
    Vector3d vo = Vector3d::orthoNormalize(forward, up);
    Vector3d right = Vector3d::crossProduct(up, forward);

#define m00 right.x
#define m01 up.x
#define m02 forward.x
#define m10 right.y
#define m11 up.y
#define m12 forward.y
#define m20 right.z
#define m21 up.z
#define m22 forward.z

    Quaternion ret;
    ret.w = sqrtf(1.0f + m00 + m11 + m22) * 0.5f;
    float w4_recip = 1.0f / (4.0f * ret.w);
    ret.x = (m21 - m12) * w4_recip;
    ret.y = (m02 - m20) * w4_recip;
    ret.z = (m10 - m01) * w4_recip;

#undef m00
#undef m01
#undef m02
#undef m10
#undef m11
#undef m12
#undef m20
#undef m21
#undef m22

    return ret;
}

Quaternion Quaternion::fromEulerAngles( Omen::Float yaw, Omen::Float pitch, Omen::Float roll )
{
    Quaternion q;

    Omen::Float c1 = cos(yaw/2);
    Omen::Float s1 = sin(yaw/2);
    Omen::Float c2 = cos(pitch/2);
    Omen::Float s2 = sin(pitch/2);
    Omen::Float c3 = cos(roll/2);
    Omen::Float s3 = sin(roll/2);
    Omen::Float c1c2 = c1*c2;
    Omen::Float s1s2 = s1*s2;

    q.w = c1c2*c3  - s1s2*s3;
    q.x = c1c2*s3  + s1s2*c3;
    q.y = s1*c2*c3 + c1*s2*s3;
    q.z = c1*s2*c3 - s1*c2*s3;
    return q;
}

Omen::Vector3d  Quaternion::toEulerAngles( const Quaternion& q )
{
    return Vector3d( atan( 2.0f*(q.w*q.x + q.y*q.z)/(1.0f-2.0f*(q.x*q.x + q.y*q.y))),
                     asin( 2.0f*(q.w*q.y - q.z*q.x) ),
                     atan( 2.0f*(q.w*q.z + q.x*q.y)/(1.0f-2.0f*(q.y*q.y + q.z*q.z) ) ) );
}
/// EOF
